#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <std_msgs/String.h> 
#include <std_msgs/Empty.h> 
#include <geometry_msgs/Twist.h>
#include<time.h>
#include <fstream> 
#include <unistd.h>
#include <chrono>
#include <ratio>

serial::Serial ser; //声明串口对象
std_msgs::String result; //定义变量用来存储串口读取到的数据 
ros::Publisher read_pub;
ros::Publisher read1_pub;
int i=0;
ros::Publisher read2_pub;
//回调函数 

using namespace std;
using namespace std::chrono;

void write_callback(const std_msgs::String::ConstPtr& msg) 
{ 
	ROS_INFO_STREAM("Writing to serial port:" <<msg->data); 
	ser.write(msg->data);   //发送串口数据(接收到其他节点的消息用这个返回给手机)
	if(result.data=="a"||i==1||i==2)  //8 (location_move.py中使用到)
	{
		i++;
	}
	if(i==3)
	{
		i=0;
	}
}  

int main (int argc, char** argv) 
{ 
	//初始化节点 
	ros::init(argc, argv, "serial_pub"); 
	//声明节点句柄 
	ros::NodeHandle nh; 
	//订阅主题，并配置回调函数 
	ros::Subscriber write_sub = nh.subscribe("/write", 1000, write_callback);
	//发布主题 
	read_pub = nh.advertise<std_msgs::String>("/read", 1000);
	read1_pub = nh.advertise<std_msgs::String>("/read1", 1000);
        read2_pub = nh.advertise<std_msgs::String>("/read2", 1000);//

	try 
	{ 
	      //设置串口属性，并打开串口 
	      ser.setPort("/dev/ttyUSB0");     //4G
	      ser.setBaudrate(115200); 
	      serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
	      ser.setTimeout(to); 
	      ser.open(); 
	} 
	catch (serial::IOException& e) 
	{ 
	      ROS_ERROR_STREAM("Unable to open port "); 
	      return -1; 
	} 
	//检测串口是否已经打开，并给出提示信息 
	if(ser.isOpen()) 
	{ 
	      ROS_INFO_STREAM("Serial Port initialized"); 
	} 
	else 
	{ 
	      return -1; 
	} 
	//指定循环的频率 
	ros::Rate loop_rate(50); 
	//std_msgs::String result; //定义变量用来存储串口读取到的数据

/*
ofstream outfile("远距离.txt"); 

//double tt = 0;
//clock_t start,end;
string test="1111";
//start = clock();


for(int j=0;j<1000;j++)
{

auto start = steady_clock::now();

for(int i=0;i<10;i++)
{

  //start = clock();
  ser.write(test);
  while(test[0] != '9' && ros::ok())
  {
    //ser.write(test);
    if(ser.available())
    {
       //cout<<ser.read(ser.available())<<endl;
       test = ser.read(ser.available());
       //if(test[0] == '9')
         //end = clock();
       
    }
  }
  //usleep(10000);
  //tt = 1000*(double)(end-start)/CLOCKS_PER_SEC;
  //cout<<i<<endl;
  //cout<<tt<<endl;
  //test = "1111";
  //outfile<<i+1<<','<<tt<<endl;

}

auto end = steady_clock::now();

//end = clock();
//tt = (double)((end-start)/CLOCKS_PER_SEC);

auto tt = duration_cast<microseconds>(end - start);

cout<<tt.count()<<endl;
outfile<<j+1<<','<<tt.count()/10000.0<<endl;

}



outfile.close();

*/

/*
	while(ros::ok()) 
	{ 

	      if(ser.available()) 
	      { 

 			if(i==0)
			{
				result.data = ser.read(ser.available()); 
			    	ROS_INFO_STREAM("Read: " << result.data);
				read_pub.publish(result); 
			}    
			if(i==1)
			{
				result.data = ser.read(ser.available()); 
			    	ROS_INFO_STREAM("Read2: " << result.data);
				read1_pub.publish(result); 
			}  
			if(i==2)
			{
				result.data = ser.read(ser.available()); 
			    	ROS_INFO_STREAM("Read2: " << result.data);
				read2_pub.publish(result); 
			}           
	      } 
	      ser.flush ();//清空串口存储空间
	      //处理ROS的信息，比如订阅消息,并调用回调函数 
	      ros::spinOnce(); 
	      loop_rate.sleep(); 

 	} 
*/

string test="";
	while(ros::ok()) 
	{ 
            if(ser.available())//
	    {
              test = ser.read();

              cout<<test<<endl;

              if(test.length() > 3)
              {
                cout<<test<<endl;
                continue;
              }
              else
              {
               if(test[0] == '1') 
	       { 
	          ser.flush ();//清空串口存储空间
                  ser.write(test);    
	       } 
               test="";
             }
	       ser.flush ();//清空串口存储空间
              }
             

	      

 	} 


}

